package src;

import edu.wpi.first.wpilibj.*;
import util.RateEncoder;

/**
 *
 * @author TJ
 */
public class Payload {
    private SpeedController shooterMotors;
    private RateEncoder shooterEncoder;
    private PIDController shooterPID;
    private Solenoid shooterActuatorOff, shooterActuatorOn, netActuatorOff, netActuatorOn;

    private int shooterMode;
    private double shooterValue;
    private Timer shooterTimer;

    public Payload() {
        shooterMotors = Robot.payloadShooterMotors;
        shooterEncoder = Robot.payloadShooterEncoder;
        shooterPID = Robot.payloadShooterPID;
        shooterActuatorOff = Robot.payloadShooterActuatorOff;
        shooterActuatorOn = Robot.payloadShooterActuatorOn;
        netActuatorOff = Robot.payloadNetActuatorOff;
        netActuatorOn = Robot.payloadNetActuatorOn;

        shooterActuatorOff.set(true);
        shooterActuatorOn.set(false);
        netActuatorOff.set(true);
        netActuatorOn.set(false);

        shooterMode = Robot.CONTROL_PID;
        shooterPID.enable();
        shooterValue = 0;
        shooterTimer = new Timer();
    }

    public void setShooterMode(int mode) {
        if(mode == Robot.CONTROL_RAW && shooterMode == Robot.CONTROL_PID) {
            shooterPID.disable();
            shooterMotors.set(0);
        } else if(mode == Robot.CONTROL_PID && shooterMode == Robot.CONTROL_RAW) {
            shooterPID.enable();
        }
        shooterMode = mode;
    }

    public void resetShooterPID() {
        shooterPID.reset();
    }
    
    public double getShooter() {
        return shooterValue;
    }

    public void setShooter(double value) {
        shooterValue = value;
        shooterPID.setSetpoint(shooterValue);
        if(shooterMode == Robot.CONTROL_RAW) {
            shooterMotors.set(shooterValue);
        }
    }

    public void setFire(boolean on) {
        shooterActuatorOff.set(!on);
        shooterActuatorOn.set(on);
    }

    public void setNet(boolean on) {
        netActuatorOff.set(!on);
        netActuatorOn.set(on);
    }
}
